/*
   DEMO WANDER
*/

#include <stdio.h>
#include <math.h>
#include <libplayerc/playerc.h>

#define PIVOT 30
#define DIST_DESIRED 0.8

float sat(float v, int ulow, int uhigh){
	if( v < ulow )
		return ulow;
	else if(v > uhigh)
		return uhigh;
}

//Set analog output
void daout(float u, playerc_position2d_t *position2d, float newspeed){
	playerc_position2d_set_cmd_vel(position2d, newspeed, 0, DTOR(u), 1);
}

//Read values from sensor (laser) and estimates x axis distance
void read_ps_var(long int *turn_left, long int *turn_right, playerc_laser_t *laser, int range){
	int j;
	for (j=0; j < range; j++){
	/*//Future use of weight, up to now just weight = 1
		l_turn_right[j] = laser->scan[j][0] * cos(laser->scan[j][1]);
		l_turn_left[j] = laser->scan[PIVOT+j][0] * cos(laser->scan[PIVOT+j][1]);
	*/
		*turn_right = laser->scan[j][0] * cos(laser->scan[j][1]);
		*turn_left = laser->scan[PIVOT+j][0] * cos(laser->scan[PIVOT+j][1]);
	}
}

int main(int argc, const char **argv)
{
  int i;
  playerc_client_t *client;
  playerc_position2d_t *position2d;
  playerc_laser_t *laser;

  // Create a client and connect it to the server.
  client = playerc_client_create(NULL, "localhost", 6665);
  if (0 != playerc_client_connect(client))
    return -1;

  // Create and subscribe to a position2d device.
  position2d = playerc_position2d_create(client, 0);
  if (playerc_position2d_subscribe(position2d, PLAYER_OPEN_MODE))
    return -1;

  laser = playerc_laser_create(client, 0);
  if (playerc_laser_subscribe(laser, PLAYERC_OPEN_MODE))
    return -1;

  // REAL Robot: Execute serveral reads before start using the robot...
  playerc_client_read(client);
  playerc_client_read(client);
  playerc_client_read(client);
  playerc_client_read(client);
  playerc_client_read(client);
  
	//Robot process variables
	int range = PIVOT - 1;				//Amount of lasers
	//no walls = 0
	//Following wall at its right = 1
	//Following wall at its left = 2
	int bool_wall = 1;					//Boolean follow wall
//	int l_turn_left[range], l_turn_right[range];	//Store x axis dist
	long int turn_right=0, turn_left=0, expected = range*DIST_DESIRED;
	int ulow=0, uhigh=4;				//Set physical limits for motor
	float newspeed = 1;
	
	//PID variables
	float K, Kold, Knew;
	float b, bold, bnew;
	int Ti, Td, Tf, Tt;
	int p1, p2, p3, p4, p5, p6;			//Constants (only 1 calculation at the beginning)
	float y, ysp;
	float v, u;
	float I;
	float h;
	float x;

	//Read values from sensor (laser) and estimates x axis distance
	read_ps_var(&turn_left, &turn_right, laser, range);


	//Compute controller coefficients
	p1 = K*b;								//set-point gain
	p2 = K + K*Td / (Tf+h);					//PD gain
	p3 = Tf / (Tf+h);						//filter constant
	p4 = K * Td * h / ((Tf+h) * (Tf+h));	//derivative gain
	p5 = K*h / Ti;							//Integral gain
	p6 = h / Tt;							//anti-windup gain


	// go into read-think-act loop
    for(;;)
    {
		playerc_client_read(client);						//Retrieve process variables
		//Bumpless parameter changes
		I = I + Kold * (bold + ysp - y) - Knew * (bnew * ysp - y);

		//Control algorithm
		ysp = DIST_DESIRED;									//Read setpoint
		read_ps_var(&turn_left, &turn_right, laser, range);	//Read process variable
		y = turn_left - turn_right;							//Convert for PID control
		v = p1 * ysp - p2 * y + x + I;						//Compute nominal  output
		u = sat(v, ulow, uhigh);							//Saturate output
		daout(u, position2d, newspeed);						//Set analog output
		x = p3 * x + p4 * y;								//Update derivative
		I = I + p5 * (ysp - y) + p6 * (u - v);				//Update integral
    }


  // Shutdown
  playerc_position2d_unsubscribe(position2d);
  playerc_position2d_destroy(position2d);
  playerc_client_disconnect(client);
  playerc_client_destroy(client);


  return 0;
}
